﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace robotGUI
{
    public enum Command
    {
        //Parameters
        CALIBRATION_DONE,
        ABILITY_ENABLE,
        OPTION_FLAGS,
        ACTIVE_PROFILE,
        CHANNEL1_FUNCTION,
        CHANNEL2_FUNCTION,
        CHANNEL3_FUNCTION,
        CHANNEL4_FUNCTION,
        PULSES_PER_METER,
        MAX_SPEED_PPS,
        PWM_DEADBAND,
        LEFTMOTORPID_KP,
        LEFTMOTORPID_KI,
        LEFTMOTORPID_KD,
        LEFTMOTORPID_MINGAIN,
        LEFTMOTORPID_MAXGAIN,
        LEFTMOTORPID_MINOUT,
        LEFTMOTORPID_MAXOUT,
        RIGHTMOTORPID_KP,
        RIGHTMOTORPID_KI,
        RIGHTMOTORPID_KD,
        RIGHTMOTORPID_MINGAIN,
        RIGHTMOTORPID_MAXGAIN,
        RIGHTMOTORPID_MINOUT,
        RIGHTMOTORPID_MAXOUT,
        PERFECT_BALANCE,
        LOST_BALANCE,
        BALANCE_CONTROLS,
        BALANCEPID_KP,
        BALANCEPID_KI,
        BALANCEPID_KD,
        BALANCEPID_MINGAIN,
        BALANCEPID_MAXGAIN,
        BALANCEPID_MINOUT,
        BALANCEPID_MAXOUT,
        POSHOLD_STRENGTH,
        POSHOLD_LIMIT,
        DRIFT_PERCENT,
        DRIFT_DELAY,
        BATT_FACTOR,
        BATT_LOW_VOLTAGE,
        LOOP_TIME,
        UPDATE_RATE,
        USER_NAME,
        DEVICE_ID,
        //Read-only parameters
        EEPROM_SIZE,
        VERSION_REQUEST,
        ABILITIES_REQUEST,
        //Immediate commands
        DRIVING_MODE,
        DATA_REQUEST,
        EXTERNAL_OUT,  //Last parameter - see LastCommand below 
        EEPROM_DIRECT,
        EEPROM_RESTORE,
        PARAMETER_REQUEST,
        COPY_PROFILE,
        SERIAL_CONTROL,
        PWM_POWER,
        SOFT_RESET,
        TONE_DURATION,
        BATT_RAW,
        NONE
    }

    public class robotMsg
    {
        //This needs to be the last parameter in the command enumeration
        public static int LastCommand = (int)Command.EXTERNAL_OUT;
        //Interlock key to prevent accidental doom
        public static byte DOOMSDAY_MAGIC = 85;
        //The raw message string
        public string Message;
        //Requested data types
        public enum DataType
        {
            REQUEST_PID_NONE = 0,
            REQUEST_PID_LMOTOR = 1,
            REQUEST_PID_RMOTOR = 2,
            REQUEST_PID_BALANCE = 3,
            REQUEST_PID = 4,
            REQUEST_RC = 8,
            REQUEST_SENSORS = 16,
            REQUEST_MOTORS = 32,
            REQUEST_TIMING = 64,
            REQUEST_MOVEMENT = 128
        }


        public string status;
        public enum Type { HB, STD, SEN, MOT, MOV, RC, PID, STAT, PAR, TIM, ERR, NULL };
        public Type msgType = Type.STD;
        public float ACC_pitch;
        public float pitchAngle;
        public float ACC_roll;
        public float rollAngle;
        public float ACC_X;
        public float ACC_Y;
        public float ACC_Z;
        public float GYR_X;
        public float GYR_Y;
        public float GYR_Z;
        public float bal_pTerm, bal_iTerm, bal_dTerm;
        public float bal_input, bal_output, bal_error, bal_setPoint;
        //public float bal_Kp, bal_Ki, bal_Kd;
        public float STD_LOOP_TIME;
        public float lastLoopUsefulTime;
        public float lastLoopTime;
        public float updateRate;
        public Int16 failsafeFlags;
        public float rcData1;
        public float rcData2;
        public float rcData3;
        public float rcData4;
        public float current_speed_R_motor;
        public long count_R;
        public float rightPWM;
        public float rm_pTerm;
        public float rm_iTerm;
        public float rm_dTerm;
        public float rm_input;
        public float rm_output;
        public float rm_setPoint;
        public float rm_error; 
        //public float rm_Kp;
        //public float rm_Ki;
        //public float rm_Kd;
        public float current_speed_L_motor;
        public long count_L;
        public float leftPWM;
        public float lm_pTerm;
        public float lm_iTerm;
        public float lm_dTerm;
        public float lm_input;
        public float lm_output;
        public float lm_setPoint;
        public float lm_error;
        //public float lm_Kp;
        //public float lm_Ki;
        //public float lm_Kd;
        public float batteryVoltage;
        public float driftPercent;
        public float driftRate;
        public float drivingMode;
        public byte dataRequest;

        public int parCommand;
        public int parAddress;
        public float parData;
        public string parString;

        public robotMsg() { Message = ""; }

        public void Update(string msg)
        {
            Message = msg;
            //Ignore empty messages
            if (Message.Length < 4)
            {
                msgType = Type.NULL;
                status = Message;
                return;
            }
            if (Message.Substring(0,4) == "STAT")
            {
                msgType = Type.STAT;
                status = Message.Substring(5, Message.Length - 6);
                return; //No need for further processing - may confuse later parsers
            }
            string[] values = Message.Split(',');
            if (values.Length > 0)
            {
                if (values[0] == "HB")
                {
                    msgType = Type.HB;
                    if (values.Length > 1)
                        float.TryParse(values[1], out drivingMode);
                    if (values.Length > 2)
                        short.TryParse(values[2], out failsafeFlags);
                    if (values.Length > 3)
                        byte.TryParse(values[3], out dataRequest);
                    if (values.Length > 4)
                        float.TryParse(values[4], out batteryVoltage);
                }

                //Parameter
                else if (values[0] == "PAR")
                {
                    msgType = Type.PAR;
                    int.TryParse(values[1], out parCommand);
                    if ((Command)parCommand == Command.USER_NAME ||
                        (Command)parCommand == Command.VERSION_REQUEST)
                        parString = Message.Substring(7, Message.Length - 8);
                    else
                    {
                        float.TryParse(values[2], out parData);
                        if (values.Length > 3) int.TryParse(values[3], out parAddress);
                    }
                }

                else if (values[0] == "SEN")
                {
                    msgType = Type.SEN;
                    if (values.Length > 1)
                        float.TryParse(values[1], out ACC_pitch);
                    if (values.Length > 2)
                        float.TryParse(values[2], out ACC_roll);
                    if (values.Length > 3)
                        float.TryParse(values[3], out ACC_X);
                    if (values.Length > 4)
                        float.TryParse(values[4], out ACC_Y);
                    if (values.Length > 5)
                        float.TryParse(values[5], out ACC_Z);
                    if (values.Length > 6)
                        float.TryParse(values[6], out GYR_X);
                    if (values.Length > 7)
                        float.TryParse(values[7], out GYR_Y);
                    if (values.Length > 8)
                        float.TryParse(values[8], out GYR_Z);
                }

                else if (values[0] == "MOT")
                {
                    msgType = Type.MOT;
                    if (values.Length > 1)
                        long.TryParse(values[1], out count_L);
                    if (values.Length > 2)
                        float.TryParse(values[2], out leftPWM);
                    if (values.Length > 3)
                        long.TryParse(values[3], out count_R);
                    if (values.Length > 4)
                        float.TryParse(values[4], out rightPWM);
                }

                else if (values[0] == "MOV")
                {
                    msgType = Type.MOV;
                    if (values.Length > 1)
                        float.TryParse(values[1], out current_speed_L_motor);
                    if (values.Length > 2)
                        float.TryParse(values[2], out current_speed_R_motor);
                    if (values.Length > 3)
                        float.TryParse(values[3], out pitchAngle);
                    if (values.Length > 4)
                        float.TryParse(values[4], out rollAngle);
                }


                else if (values[0] == "RC")
                {
                    msgType = Type.RC;
                    if (values.Length > 1)
                        float.TryParse(values[1], out rcData1);
                    if (values.Length > 2)
                        float.TryParse(values[2], out rcData2);
                    if (values.Length > 3)
                        float.TryParse(values[3], out rcData3);
                    if (values.Length > 4)
                        float.TryParse(values[4], out rcData4);
                    if (values.Length > 5)
                        float.TryParse(values[5], out driftPercent);
                    if (values.Length > 6)
                        float.TryParse(values[6], out driftRate);
                }
                else if (values[0] == "PID" && values.Length == 8)
                {
                    msgType = Type.PID;
                    if ((dataRequest & 7) == (byte)DataType.REQUEST_PID_LMOTOR)
                    {
                        float.TryParse(values[1], out lm_input);
                        float.TryParse(values[2], out lm_setPoint);
                        float.TryParse(values[3], out lm_error);
                        float.TryParse(values[4], out lm_pTerm);
                        float.TryParse(values[5], out lm_iTerm);
                        float.TryParse(values[6], out lm_dTerm);
                        float.TryParse(values[7], out lm_output);
                    }
                    if ((dataRequest & 7) == (byte)DataType.REQUEST_PID_RMOTOR)
                    {
                        float.TryParse(values[1], out rm_input);
                        float.TryParse(values[2], out rm_setPoint);
                        float.TryParse(values[3], out rm_error);
                        float.TryParse(values[4], out rm_pTerm);
                        float.TryParse(values[5], out rm_iTerm);
                        float.TryParse(values[6], out rm_dTerm);
                        float.TryParse(values[7], out rm_output);
                    }
                    if ((dataRequest & 7) == (byte)DataType.REQUEST_PID_BALANCE)
                    {
                        float.TryParse(values[1], out bal_input);
                        float.TryParse(values[2], out bal_setPoint);
                        float.TryParse(values[3], out bal_error);
                        float.TryParse(values[4], out bal_pTerm);
                        float.TryParse(values[5], out bal_iTerm);
                        float.TryParse(values[6], out bal_dTerm);
                        float.TryParse(values[7], out bal_output);
                    }
                }
                else if (values[0] == "TIM")
                {
                    msgType = Type.TIM;
                    float.TryParse(values[1], out lastLoopUsefulTime);
                    float.TryParse(values[2], out lastLoopTime);
                }
                else if (values[0] == "PID" && values.Length == 19)
                {
                    msgType = Type.PID;
                    float.TryParse(values[1], out current_speed_R_motor);
                    long.TryParse(values[2], out count_R);
                    float.TryParse(values[3], out rm_pTerm);
                    float.TryParse(values[4], out rm_iTerm);
                    float.TryParse(values[5], out rm_dTerm);
                    float.TryParse(values[6], out rm_output);
                    //float.TryParse(values[7], out rm_Kp);
                    //float.TryParse(values[8], out rm_Ki);
                    //float.TryParse(values[9], out rm_Kd);
                    float.TryParse(values[10], out current_speed_L_motor);
                    long.TryParse(values[11], out count_L);
                    float.TryParse(values[12], out lm_pTerm);
                    float.TryParse(values[13], out lm_iTerm);
                    float.TryParse(values[14], out lm_dTerm);
                    float.TryParse(values[15], out lm_output);
                    //float.TryParse(values[16], out lm_Kp);
                    //float.TryParse(values[17], out lm_Ki);
                    //float.TryParse(values[18], out lm_Kd);
                }
                else if (float.TryParse(values[0], out ACC_pitch))
                {
                    msgType = Type.STD;
                    if (values.Length > 1)
                        float.TryParse(values[1], out pitchAngle);
                    if (values.Length > 2)
                        float.TryParse(values[2], out ACC_X);
                    if (values.Length > 3)
                        float.TryParse(values[3], out ACC_Z);
                    if (values.Length > 4)
                        float.TryParse(values[4], out GYR_Y);
                    if (values.Length > 5)
                        float.TryParse(values[5], out bal_pTerm);
                    if (values.Length > 6)
                        float.TryParse(values[6], out bal_iTerm);
                    if (values.Length > 7)
                        float.TryParse(values[7], out bal_dTerm);
                    if (values.Length > 8)
                        float.TryParse(values[8], out bal_output);
                    if (values.Length > 9)
                        float.TryParse(values[9], out bal_error);
                    if (values.Length > 10)
                        float.TryParse(values[10], out bal_setPoint);
                    /*if (values.Length > 11)
                        float.TryParse(values[11], out bal_Kp);
                    if (values.Length > 12)
                        float.TryParse(values[12], out bal_Kp);
                    if (values.Length > 13)
                        float.TryParse(values[13], out bal_Ki);
                    if (values.Length > 14)
                        float.TryParse(values[14], out bal_Kd);
                    */
                    if (values.Length > 15)
                        float.TryParse(values[15], out STD_LOOP_TIME);
                    if (values.Length > 16)
                        float.TryParse(values[16], out lastLoopUsefulTime);
                    if (values.Length > 17)
                        float.TryParse(values[17], out lastLoopTime);
                    if (values.Length > 18)
                        float.TryParse(values[18], out updateRate);
                    if (values.Length > 19)
                        float.TryParse(values[19], out rollAngle);
                }
                else
                {
                    msgType = Type.ERR;
                    status = Message;
                }
            }
        }
    }
}

